package simulator;
import java.util.*;
/**
 * Environment consists of all objects in the world, 
 * Along with their placements and the placement of
 * the robot. All interactions in world occur based on this data 
 * @author kroo
 */

public class Environment {
  // list of all objects in the environment
  private ArrayList<PhysicalObject> objects = new ArrayList<PhysicalObject>();

  // access to the environment (see sharedInstance())
  protected static Environment sharedEnvironment = new Environment();

  // current location & orientation of the robot
  protected double robotX, robotY, robotAngle;


  public void setStart(double x, double y, double o) {
    robotX=x;
    robotY=y;
    robotAngle = o;
  }
  /**
   * Default Constructor -- you should probably access the shared instance
   * instead.
   *
   * @see sharedInstance
   */
  public Environment() {}

  /**
   *  Provides static access to the current environment object.
   *  Note: this is not safe in situations when more than one simulation is
   *  running in the same JVM.
   *
   *  @return the current environment object
   */
  public static Environment sharedInstance() { return sharedEnvironment; }

  /**
   *  Add an object to the environment.
   *
   * @param o the object to be added
   * @see PhysicalObject
   */
  public void add(PhysicalObject o) {
    objects.add(o);
  }

  /**
   * Get the i'th object in the environment (convenience method)
   *
   * @return the i'th object in the environment
   * @param i index of the object
   */
  public PhysicalObject get(int i) {
    return objects.get(i);
  }

  /**
   * Retreives the array of PhysicalObjects in the environment
   *
   * @return every object in the environment
   */
  public ArrayList<PhysicalObject> getObjects() {
    return objects;
  }

  /**
   * Removes an object from the environment
   *
   * @param o the object to be removed
   */
  public void remove(PhysicalObject o) {
    objects.remove(o);
  }

  // These are local objects for ranged sensors like sonar
  /** 
   * NOTE
   * This method will return all known local objects,
   * which simulated sensors will need to filter by the actual
   * reach of the hardware
   */
     public ArrayList<PhysicalObject> getLocalObjects(double x,
  						   double y,
  						   double radius) {
  						   

    /*ArrayList<PhysicalObject> locals = new ArrayList<PhysicalObject>();
    for(int i=0; i<objects.size(); i++) {
      PhysicalObject local = objects.get(i);
	  
      if(Math.sqrt( (x - local.getX()) * (x - local.getX()) +
      		    (y - local.getY()) * (y - local.getY()) )
      		    <= radius){
				
      locals.add(local);
      }
	  
    }*/
    return objects;
  }
  
  /**
   * Updates the bot's location and orientation (called from Simulator)
   *
   * @param x the x component of the bots location
   * @param y the y component of the bots location
   * @param angle the bots 2d orientation (in radians)
   * @see Simulator
   */
  public void updateLocation(double x, double y, double angle){
    robotX=x;
    robotY=y;
    robotAngle=angle;
  }
  
  /**
   * Returns the robots current x location
   *
   * @return the robot's x component of its location
   */
  public double getRobotX(){
	return robotX;
  }
  
  
  /**
   * Returns the robots current y location
   *
   * @return the robot's y component of its location
   */
  public double getRobotY(){
	return robotY;
  }
   
  /**
   * Returns the robots current 2D orientation
   *
   * @return the robot's orientation (right = 0, ccw, units are radians)
   */
  public double getRobotAngle(){
	return robotAngle;
  }
}